#!/usr/bin/env python
# Shenzhen Joyton RD - Awesome 2022-12-08

import rospy
from std_msgs.msg import Float32
import  msp
import serial

rospy.loginfo("------ MSP Vol Node ------")

def msp_ros_node_run():
    portname = "/dev/ttyTHS1"
    baudrate = 115200
    batt_info_freq = 10
    ## init node
    rospy.init_node('msp_ros_node', anonymous=True)

    rospy.loginfo(rospy.get_param_names())

    ## read private param
    portname = rospy.get_param("~portname")
    baudrate = rospy.get_param("~baudrate")
    batt_info_freq = rospy.get_param("~batt_info_freq")
    if  batt_info_freq>=200: batt_info_freq = 200

    rospy.loginfo("Port: %s, BD: %d, Freq:%dHz" % (portname, baudrate, batt_info_freq))

    serial_fd = serial.Serial(portname, baudrate, timeout=0)
    msp_obj = msp.msp(serial_fd)
    msp_obj.add_msg_request(msp.MSP_ANALOG, batt_info_freq)
    msp_obj.add_msg_request(msp.MSP_ATTITUDE, 2)
    msp_obj.add_msg_request(msp.MSP_IDENT, 0.1)
    rospy.loginfo("MSP Request List: ", msp_obj.request_list)

    msp_obj.msp_start_process(msp_obj) 

    ## define publisher
    batt_vol_pub = rospy.Publisher('battery_voltage',  Float32,  queue_size=1)
    rate = rospy.Rate(batt_info_freq) # 10hz

    bat_vol_valid = False
    while not rospy.is_shutdown():
        if msp_obj.msp_comm_timeout():
            vbat = 0.0
            bat_vol_valid = False
            rospy.loginfo('MSP ROS: Battery invalid')
        else:
            vbat = msp_obj.vbat
        
        if not bat_vol_valid and (vbat>0.1):
            bat_vol_valid = True
            rospy.loginfo('MSP ROS: Battery valid')
        batt_vol_pub.publish(vbat)
        #rospy.loginfo("pub vbat: %.1fV" % vbat)
        rate.sleep()

if __name__=="__main__":
   try:
       msp_ros_node_run()
   except:
      rospy.loginfo("Run Error!")
      pass
